ROS2 node

We provide a unique ROS2 node to use WOLF in ROS2. This node is generic in the sense that it will serve many different robotics projects without modification. This means that all the user has to do is:

  • Write a YAML file or a set of YAML files specifying all the problem parameters.

  • Write a launch file for your application specifying the paths to your YAML files and installed WOLF plugins and subscribers.

  • Launch and enjoy.

See also

See the YAML configuration section for more information on configuring a ROS2 application with WOLF.

This unique ROS2 node is implemented in the wolf_ros2_node package.

Description of the node

A quick overview of the ROS node follows:

namespace wolf
{
class WolfNode : public rclcpp::Node
{
    // reentrant callback group (apart from default mutually exclusive)
    rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_;
    // timers
    rclcpp::TimerBase::SharedPtr timer_print_ptr_, timer_sub_watchdog_ptr_;
    // subscribers
    std::list<SubscriberBasePtr> subscribers_;
    // publishers
    std::list<PublisherBasePtr> publishers_;
    // sensor switcher
    SensorSwitcherPtr sensor_switcher_;

  protected:
    YAML::Node       params_;       ///< params
    ProblemPtr       problem_ptr_;  ///< wolf problem
    SolverManagerPtr solver_;       ///< solver

    // profiling
    bool                                                        profiling_;
    std::ofstream                                               profiling_file_;
    std::chrono::time_point<std::chrono::high_resolution_clock> start_experiment_;

    // print
    bool   print_problem_;
    double print_period_;
    int    print_depth_;
    bool   print_factored_by_, print_metric_, print_state_blocks_;

  public:
    WolfNode();
    virtual ~WolfNode();
    void                             init();
    void                             print();
    void                             subscribersWatchdog();
    void                             createProfilingFile();
    rclcpp::CallbackGroup::SharedPtr getReentrantCallbackGroup()
    {
        return reentrant_callback_group_;
    };
};

}  // namespace wolf

where the principal bits are briefly described below:

The node constructor & initialize()

The node constructor uses WOLF autoconf to:

  1. Read the path of the main YAML file provided through the ROS parameter yaml_file_path.

  2. Load the YAML file and search the required plugins and packages.

  3. Load all required WOLF plugins and ROS2 packages and store their schema folders for validation.

  4. Validate the YAML configuration against the loaded schemas.

  5. Configure the WOLF problem as specified (dimension, sensors, processors, tree manager, map…)

  6. Configure a solver for the factor graph.

  7. Configure other features such as node rate, profiling options and printing options.

Then, the initialize() method is called to:

  1. Create a series of ROS2 subscribers with callbacks, one per sensor, as specified in the YAML config file.

  2. Create a series of ROS2 publishers with topic and specified frequency in the YAML config file.

  3. Create a sensor switcher to enable/disable sensors on the fly.

solve() & solveLoop()

The solve() method calls the solveLoop() periodically to optimize the factor graph and if specified, compute the covariance of parts of the state. It is executed in an dedicated thread with high priority to ensure that the solver is not delayed by other processes.

The main() routine

In ǹode.cpp, you can find the main() routine:

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto wolf_node = std::make_shared<wolf::WolfNode>();
    wolf_node->init();

    rclcpp::spin(wolf_node);

    rclcpp::shutdown();

    return 0;
}

Some important remarks:

  • The subscribers are called sequentially in the main thread. Note that inside the subscribers, a capture of the corresponding type will be created and processed.

  • The publishers have their dedicated threads.